/*
 * motor.c
 *
 *  Created on: 2023-06-27
 *      Author: chenss
 */

#include "motor.h"
#include "hw.h"
#include "shell.h"
#include "tmc222x.h"
#include "message_queue.h"

#define _PWM PWM0

#define CH_NUM  0
#define CH_MASK PWM_CH_0_MASK

#define PWM_CLK 22118400UL
#define PWM_PRESCALE 400
#define COUNTER_CLK (PWM_CLK / PWM_PRESCALE)

#define FREQ_LOWER_LIMIT (COUNTER_CLK/0xffff + 1)
#define FREQ_UPPER_LIMIT COUNTER_CLK

#define FREQ_MIN 80
#define FREQ_MAX 1200
#define FREQ_CHANGE_MAX 100

#if FREQ_MIN < FREQ_LOWER_LIMIT
#   error FREQ_MIN Error!
#endif

#if FREQ_MAX > FREQ_UPPER_LIMIT
#   error FREQ_MAX Error!
#endif

#define POSITION_MAX 4000

#define SGTHRS 2

static struct _Ctrl
{
    bool driverReady;
    int target;
    int distance;
    int position;
    int pass;
    int acc_distance;
    int dec_distance;
    int freq;
    int freq_max;
    int freq_change;
    Callback_Info_t arrival;
    Callback_Info_t diag;
}_ctrl;

static int _cycle_target;

static void _PWMInit(void);
static inline void _SetMotorEnable(bool isEnable);
static inline void _SetMotorStop(bool isStop);
static int _OnDefaultDiag(void *arg);
static int _OnDefaultArrival(void *arg);

void Motor_Init(void)
{
    nbdebug("%s\n", __func__);
    memset(&_ctrl, 0, sizeof(_ctrl));

    _cycle_target = 0;
    _ctrl.freq = FREQ_MIN;
    _ctrl.freq_max = FREQ_MAX;
    Motor_SetDiagCallback(_OnDefaultDiag, NULL);
    Motor_SetArriveCallback(_OnDefaultArrival, NULL);

    _PWMInit();
    int ret = TMC222x_Init();
    if(ret != 0)
    {
        nbprintf("motor driver not ready!\n");
        _SetMotorEnable(false);
        return;
    }

    _SetMotorEnable(true);
    _ctrl.driverReady = true;

    Motor_SetTarget(-4000);
}

static void _PWMInit(void)
{
    uint32_t cnr = COUNTER_CLK / FREQ_MIN;
    uint32_t cmr = 50 * cnr / 100;

    PWM_SET_PRESCALER(_PWM, CH_NUM, PWM_PRESCALE - 1);
    PWM_SET_CNR(_PWM, CH_NUM, cnr - 1);
    PWM_SET_CMR(_PWM, CH_NUM, cmr - 1);

#if 0
    // set PWM to up count type(edge aligned)
    (_PWM)->CTL1 = ((_PWM)->CTL1 & ~(PWM_CTL1_CNTTYPE0_Msk << (2 * CH_NUM))) | (0UL << (2 * CH_NUM));
    // set PWM to auto-reload mode
    (_PWM)->CTL1 &= ~(PWM_CTL1_CNTMODE0_Msk << CH_NUM);
#endif

    // 零点为低电平、比较点为高电平
    (_PWM)->WGCTL0 &= ~((PWM_WGCTL0_PRDPCTL0_Msk | PWM_WGCTL0_ZPCTL0_Msk) << (CH_NUM * 2));
    (_PWM)->WGCTL0 |= (PWM_OUTPUT_LOW << (CH_NUM * 2 + PWM_WGCTL0_ZPCTL0_Pos));
    (_PWM)->WGCTL1 &= ~((PWM_WGCTL1_CMPDCTL0_Msk | PWM_WGCTL1_CMPUCTL0_Msk) << (CH_NUM * 2));
    (_PWM)->WGCTL1 |= (PWM_OUTPUT_HIGH << (CH_NUM * 2 + PWM_WGCTL1_CMPUCTL0_Pos));
    PWM_EnableOutput(_PWM, CH_MASK);

    PWM_ClearPeriodIntFlag(_PWM, CH_NUM);
    PWM_EnablePeriodInt(_PWM, CH_NUM, 0);
    NVIC_EnableIRQ(PWM0P0_IRQn);
}

static inline void _SetMotorForware(bool isForware)
{
    HW_Level_t level = isForware ? HW_Level_High : HW_Level_Low;

    HW_Set(HW_Out_DIR, level);
}

static inline bool _IsSettedMotorForware(void)
{
    return (HW_GetSetted(HW_Out_DIR) == HW_Level_High) ? true : false;
}

static int _OnReachOrigin(void *arg)
{
    int target = (int)arg;

    printf("reach origin\n");
    TMC222x_WriteReg(TMC_SGTHRS, SGTHRS);
    HAL_Delay(200);

    return Motor_SetTarget(target);
}

static int _OnDefaultDiag(void *arg)
{
    _SetMotorStop(true);
    mprintf("DIAG Active!\n");

    if(_ctrl.target < 0)
    {
        _ctrl.position = 0;
        MsgQueue_CallLater(_OnReachOrigin, (void *)160);
    }
    else
    {
        _SetMotorEnable(false);
        _ctrl.driverReady = false;
        mprintf("Motor Stall!\n");
    }

    return 0;
}

void Motor_SetDiagCallback(Callback_t callback, void *ctx)
{
    Callback_Info_t *info = & _ctrl.arrival;

    if(callback != NULL)
    {
        info->callback = callback;
        info->arg = ctx;
    }
    else
    {
        info->callback = _OnDefaultDiag;
        info->arg = NULL;
    }

    HW_SetExtiCallback(HW_Exti_DIAG, info->callback, info->arg, HW_EXTI_FLAG_REALTIME);
}

static inline void _SetMotorEnable(bool isEnable)
{
    HW_State_t state = isEnable ? HW_State_Inactive : HW_State_Active;

    HW_Config(HW_Out_ENN, state);
}

static inline void _SetMotorStop(bool isStop)
{
    if(isStop)
    {
        BIT_CLR(_PWM->CNTEN, CH_MASK);
    }
    else
    {
        BIT_SET(_PWM->CNTCLR, CH_MASK);
        BIT_SET(_PWM->CNTEN, CH_MASK);
    }
}

static inline void _SetPwmFreq(unsigned int freq)
{
    uint32_t cnr = COUNTER_CLK / freq;
    uint32_t cmr = 50 * cnr / 100;

    PWM_SET_CNR(_PWM, CH_NUM, cnr - 1);
    PWM_SET_CMR(_PWM, CH_NUM, cmr - 1);
}

static int _DoSetTarget(void *arg)
{
    int target = (int)arg;

    HAL_Delay(200);
    return Motor_SetTarget(target);
}

static int _OnDefaultArrival(void *arg)
{
    int ret = 0;

    if(_cycle_target != 0)
    {
        int target = (_ctrl.position == _cycle_target) ? 160 : _cycle_target;

        MsgQueue_CallLater(_DoSetTarget, (void *)target);
    }

    return ret;
}

void Motor_SetArriveCallback(const Callback_t callback, void *ctx)
{
    if(callback != NULL)
    {
        _ctrl.arrival.callback = callback;
        _ctrl.arrival.arg = ctx;
    }
    else
    {
        _ctrl.arrival.callback = _OnDefaultArrival;
        _ctrl.arrival.arg = NULL;
    }
}

int Motor_SetTarget(int position)
{
    int ret = 0;

    if(! _ctrl.driverReady)
    {
        return -1;
    }

    if(position > POSITION_MAX)
    {
        return -2;
    }

    if(_ctrl.position == position)
    {
        _SetMotorStop(true);
        return 0;
    }

    bool isForware = (position >= _ctrl.position) ? true : false;

    _ctrl.target = position;
    _ctrl.distance = abs(_ctrl.target - _ctrl.position);
    _ctrl.pass = 0;
    _ctrl.freq_change = FREQ_CHANGE_MAX;
    _ctrl.acc_distance = _ctrl.distance;
    _ctrl.dec_distance = _ctrl.acc_distance;
    _SetPwmFreq(_ctrl.freq);

    _SetMotorEnable(true);
    _SetMotorForware(isForware);
    _SetMotorStop(false);

    return ret;
}

void PWM0P0_IRQHandler(void)
{
    bool isForware;

    if(PWM_GetPeriodIntFlag(_PWM, CH_NUM))
    {
        PWM_ClearPeriodIntFlag(_PWM, CH_NUM);

        isForware = _IsSettedMotorForware();
        _ctrl.position += isForware ? 1 : -1;
        _ctrl.pass ++;
        if(_ctrl.pass >= _ctrl.distance)
        {
            _SetMotorStop(true);
            _ctrl.freq = MIN(_ctrl.freq, FREQ_MIN);
            _ctrl.arrival.callback(_ctrl.arrival.arg);
            return;
        }

        if(_ctrl.pass < _ctrl.acc_distance) // 加速阶段
        {
            _ctrl.freq += _ctrl.freq_change;
            if(_ctrl.freq >= _ctrl.freq_max)
            {
                _ctrl.freq = _ctrl.freq_max;
                _ctrl.acc_distance = _ctrl.pass;
                _ctrl.dec_distance = _ctrl.acc_distance;
                _ctrl.freq_change = 1;
//                mprintf("AccDst: %d\n", _ctrl.acc_distance);
            }

            if(_ctrl.freq_change > 2)
            {
                _ctrl.freq_change -= 2;
            }

            _SetPwmFreq(_ctrl.freq);
        }
        else if((_ctrl.distance - _ctrl.pass) < _ctrl.dec_distance) // 减速阶段
        {
            if(_ctrl.freq > FREQ_MIN)
            {
                _ctrl.freq -= 2;
                if(_ctrl.freq < FREQ_MIN)
                {
                    _ctrl.freq = FREQ_MIN;
                }
            }

            _SetPwmFreq(_ctrl.freq);
        }
    }
}

static int _CmdMotor(void *stream, int argc, char *argv[])
{
    int ret = 0;

    if(argc < 2)
    {
        chprintf(stream, "target: %d\n", _ctrl.target);
        chprintf(stream, "position: %d\n", _ctrl.position);
        chprintf(stream, "max freq: %u\n", _ctrl.freq_max);
    }
    else
    {
        int target = atoi(argv[1]);

        if(argc >= 3)
        {
            _ctrl.freq_max = (unsigned int)atoi(argv[2]);
            if(_ctrl.freq_max < FREQ_MIN)
            {
                _ctrl.freq = _ctrl.freq_max;
            }
        }

        chprintf(stream, "set target ");
        _cycle_target = (argc >= 4) ? target : 0;
        ret = Motor_SetTarget(target);
        if(ret == 0)
        {
            chprintf(stream, "success.\n");
        }
        else
        {
            chprintf(stream, "fail: %d\n", ret);
        }
    }

    return ret;
}

SHELL_CMD(m, _CmdMotor, "show or set motor target position", "[<target position> [<max freq>]]");
